#ifndef ARMMOVER_H_
#define ARMMOVER_H_

#include "ros/ros.h"

#include "std_msgs/Float64.h"
#include "/opt/ros/fuerte/stacks/dynamixel_motor/dynamixel_msgs/msg_gen/cpp/include/dynamixel_msgs/JointState.h"
// #include "sensor_msgs/JointState.h" // replace previous line if uses relative location

#include <boost/thread.hpp>
namespace std {
using boost::mutex;
using boost::condition_variable;
using boost::unique_lock;
using boost::thread;
}

/**
 * Moves robot's arm towards point in real world (given in cm) and allows grabbing an object
 * by closing the fingers. Furthermore, allows moving each joint separately.
 */
class ArmMover {

private:

	// ros management objects
	ros::NodeHandle m_nodeHandler;
	ros::Rate m_loop_rate;

	// Publishers
	ros::Publisher m_arm_upper_elbow_pub;
	ros::Publisher m_arm_lower_elbow_pub;
	ros::Publisher m_arm_shoulder_pub;
	ros::Publisher m_arm_wrist_pub;
	ros::Publisher m_arm_base_rotation_pub;
	ros::Publisher m_arm_left_finger_pub;
	ros::Publisher m_arm_right_finger_pub;

	// Subscribers
	ros::Subscriber m_arm_right_finger_state_sub;
	ros::Subscriber m_arm_left_finger_state_sub;

	// Messages (maybe need only one)
	std_msgs::Float64 m_arm_upper_elbow_msg;
	std_msgs::Float64 m_arm_lower_elbow_msg;
	std_msgs::Float64 m_arm_shoulder_msg;
	std_msgs::Float64 m_arm_wrist_msg;
	std_msgs::Float64 m_arm_base_rotation_msg;
	std_msgs::Float64 m_arm_left_finger_msg;
	std_msgs::Float64 m_arm_right_finger_msg;

	// used for synchronized access to data
	std::mutex m_mutex;

	// load on each finger
	float m_right_finger_load;
	float m_left_finger_load;

	// position for each finger
	float m_right_finger_cur_pos;
	float m_left_finger_cur_pos;

	/**
	 * Moves the arm in vertical axis. \n
	 * Only shoulder and upper_elbow joints are involved in this move. \n
	 * Calculates vertical triangle angles and passes commands to move the
	 * motors.
	 * @param front frontal distance (cm) from base-point to real world point
	 * @param vertical vertical distance (cm) from base-point to real world point
	 * @return True if the arm moved successfully, that is given point (front,vertical) \n
	 * is in arm's range. False if the point is out of arm's range.
	 */
	bool moveVertical(double front, double vertical);

	/**
	 * Moves the arm in horizontal axis. \n
	 * Base rotation is moved according to given point. \n
	 * Calculates horizontal triangle angles and passes commands to move the motor.
	 * @param front frontal distance (cm) from base-point to real world point
	 * @param horizontal horizontal distance (cm) from base-point to real world point
	 */
	void moveHorizontal(double frontal, double horizontal);

	/**
	 * Calculates triangle angles according to three sides (a;b;c) by law of cosines (the third \n
	 * side (c) is calculated according to given coordinates (cord_i,cord_j)).
	 * @param[in] cord_i coordinate of target point in real world (cm)
	 * @param[in] cord_j coordinate of target point in real world (cm)
	 * @param[in] a first triangle side (cm)
	 * @param[in] b second triangle side (cm)
	 * @param[out] alpha first angle of triangle
	 * @param[out] gamma second angle of triangle
	 */
	void calcAngle(double cord_i, double cord_j, double a, double b, double* alpha,double* gamma);

	/**
	 * Calculates distance to point (x,y) in centimeters from (0,0)
	 * @param x coordinate of target point in real world (cm)
	 * @param y coordinate of target point in real world (cm)
	 * @return distance in centimeters from zero-point (0,0) to (x,y) point
	 */
	double getDistance(double x, double y);

	/**
	 * Normalizes angle in radians to motor position angle (also rad)
	 * @param alpha angle in radians
	 * @return motor position (rad)
	 */
	double getShoulderMotorPosition(double alpha);

	/**
	 * Normalizes angle in radians to motor position angle (also rad)
	 * @param alpha angle in radians
	 * @return motor position (rad)
	 */
	double getBaseRotationMotorPosition(double alpha);

	/**
	 * Normalizes angle in radians to motor position angle (also rad)
	 * @param alpha angle in radians
	 * @return motor position (rad)
	 */
	double getElbowMotorPosition(double gamma);

public:

	/**
	 * Constructor- initializes publishers to arm joints topics. \n
	 * Initializes subscribers to topics of fingers' motor states
	 */
	ArmMover();

	/**
	 * Publishes command to UpperElbow joint to move to value (in radians)
	 * @param value absolute motor position (radians)
	 */
	void MoveUpperElbow(double value);

	/**
	 * Publishes command to LowerElbow joint to move to value (in radians)
	 * @param value absolute motor position (radians)
	 */
	void MoveLowerElbow(double value);

	/**
	 * Publishes command to Shoulder joint to move to value (in radians)
	 * @param value absolute motor position (radians)
	 */
	void MoveShoulder(double value);

	/**
	 * Publishes command to Wrist joint to move to value (in radians)
	 * @param value absolute motor position (radians)
	 */
	void MoveWrist(double value);

	/**
	 * Publishes command to BaseRotation joint to move to value (in radians)
	 * @param value absolute motor position (radians)
	 */
	void RotateBase(double value);

	/**
	 * Publishes command to LeftFinger joint to move to value (in radians)
	 * @param value absolute motor position (radians)
	 */
	void MoveLeftFinger(double value);

	/**
	 * Publishes command to RightFinger joint to move to value (in radians)
	 * @param value absolute motor position (radians)
	 */
	void MoveRightFinger(double value);

	/**
	 * Takes point in real world in centimeters and moves the arm
	 * according to base-point (base-point is point (0,0,0) where arm is connected to the body)
	 * @param front frontal distance (cm) from base-point to real world point
	 * @param horizontal horizontal distance (cm) from base-point to real world point
	 * @param vertical vertical distance (cm) from base-point to real world point
	 * @return True if the (front,horizontal,vertical) point (in cm) is in arm's range. False if not, that is
	 * trigonometric equations returns NaN values, because arm's length is constant.
	 */
	bool moveArm(double front, double horizontal, double vertical);

	/**
	 * Closes the fingers until pressure on each not exceeds threshold value or
	 * until fingers were not completely closed
	 * @return True if and only if the fingers hold the object (the object was caught)
	 */
	bool squeezeObject();

	/**
	 * Moves arm to default start position (Contains set of default commands).
	 * Necessary for seeing the object because of position of Kinect.
	 */
	void setStartPosition();

	/**
	 * Places the arm in default for moving position.
	 * Same as \def setStartPosition, but keeps fingers close to hold the object and
	 * still see the world with Kinect.
	 */
	void liftObject();

	/**
	 * Moves the right finger from middle point till right end
	 */
	void openRightFinger();

	/**
	 * Moves the right finger from middle point to specified motor position value
	 * @param value the finger motor position
	 */
	void openRightFinger(float value);

	/**
	 * Moves the left finger from middle point till left end
	 */
	void openLeftFinger();

	/**
	 * Moves the left finger from middle point to specified motor position value
	 * @param value the finger motor position
	 */
	void openLeftFinger(float value);

	/**
	 * Moves the right finger to middle point till end
	 */
	void closeRightFinger();

	/**
	 * Moves the right finger from right end point to specified motor position value
	 * @param value the finger motor position
	 */
	void closeRightFinger(float value);

	/**
	 * Moves the left finger to middle point till end
	 */
	void closeLeftFinger();

	/**
	 * Moves the left finger from left end point to specified motor position value
	 * @param value the finger motor position
	 */
	void closeLeftFinger(float value);

	/**
	 * Opens the fingers till end
	 */
	void openFingers();

	/**
	 * Callback for /right_finger_controller/state. \n
	 * Given a right finger motor states (load, position etc.) that saved to \n
	 * class' members to feature use
	 * @param msg motor state RosMessage
	 */
	void rightFingerStateCb(const dynamixel_msgs::JointState::ConstPtr& msg);

	/**
	 * Callback for /right_finger_controller/state. \n
	 * Given a left finger motor states (load, position etc.) that saved to \n
	 * class' members to feature use
	 * @param msg motor state RosMessage
	 */
	void leftFingerStateCb(const dynamixel_msgs::JointState::ConstPtr& msg);
};

#endif

